using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.BulletX.Dynamics
{
	[Flags]
	public enum SolverMode
	{
		None = 0,
		RandomizeOrder = 1,
		FrictionSeperate = 2,
		UseWarmstarting = 4,
		CacheFriendly = 8,
	}

	public class SequentialImpulseConstraintSolver : IConstraintSolver
	{
		private static int _totalContactPoints = 0;

		private SolverMode _solverMode;
		private int _totalCpd = 0;
		private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
		private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];

		private float _penetrationResolveFactor = 0.9f;
		private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>();
		private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>();
		private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>();

		private const int _sequentialImpulseMaxSolverPoints = 16384;
		private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];
		private static long _seed2 = 0;

		public SequentialImpulseConstraintSolver()
		{
			_solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly;
			PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;

			//initialize default friction/contact funcs
			int i, j;
			for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)
				for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)
				{

					_contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision;
					_frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction;
				}
		}

		public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }
		public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }
		protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }
		public static long RandSeed { get { return _seed2; } set { _seed2 = value; } }

		///<summary>
		/// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
		/// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
		///</summary>
		public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)
		{
			_contactDispatch[typeA, typeB] = func;
		}

		/// <summary>
		/// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
		/// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
		///</summary>
		public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)
		{
			_frictionDispatch[typeA, typeB] = func;
		}

		protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
		{
			float maxImpulse = 0;

			Vector3 color = new Vector3(0, 1, 0);
			if (cp.Distance <= 0)
			{
				if (iter == 0)
					if(debugDraw != null)
						debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);

				ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
				float impulse = cpd.ContactSolverFunc(
					bodyA, bodyB,
					cp,
					info);

				if (maxImpulse < impulse)
					maxImpulse = impulse;
			}
			return maxImpulse;
		}

		protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
		{
			return Solve(bodyA, bodyB, cp, info, iter, null);
		}

		protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
		{
			float maxImpulse = 0;

			Vector3 color = new Vector3(0, 1, 0);
			if (cp.Distance <= 0)
			{
				if (iter == 0)
					if (debugDraw != null)
						debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);

				float impulse = ContactConstraint.ResolveSingleCollisionCombined(
					bodyA, bodyB,
					cp,
					info);

				if (maxImpulse < impulse)
					maxImpulse = impulse;
			}
			return maxImpulse;
		}

		protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
		{
			Vector3 color = new Vector3(0, 1, 0);

			if (cp.Distance <= 0)
			{

				ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
				cpd.FrictionSolverFunc(
					bodyA, bodyB,
					cp,
					info);
			}
			return 0;
		}

		protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
		{
			RigidBody body0 = manifold.BodyA as RigidBody;
			RigidBody body1 = manifold.BodyB as RigidBody;


			//only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
			{
				manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);

				int numpoints = manifold.ContactsCount;

				_totalContactPoints += numpoints;

				Vector3 color = new Vector3(0, 1, 0);
				for (int i = 0; i < numpoints; i++)
				{
					ManifoldPoint cp = manifold.GetContactPoint(i);
					if (cp.Distance <= 0)
					{
						Vector3 pos1 = cp.PositionWorldOnA;
						Vector3 pos2 = cp.PositionWorldOnB;

						Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
						Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;


						//this jacobian entry is re-used for all iterations
						JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
							MatrixOperations.Transpose(body1.CenterOfMassTransform),
							rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
							body1.InvInertiaDiagLocal, body1.InverseMass);

						float jacDiagAB = jac.Diagonal;

						ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
						if (cpd != null)
						{
							//might be invalid
							cpd.PersistentLifeTime++;
							if (cpd.PersistentLifeTime != cp.LifeTime)
							{
								//printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
								cpd = new ConstraintPersistentData();
								cpd.PersistentLifeTime = cp.LifeTime;

							}
						}
						else
						{

							cpd = new ConstraintPersistentData();
							_totalCpd++;
							//printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
							cp.UserPersistentData = cpd;
							cpd.PersistentLifeTime = cp.LifeTime;
							//printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());

						}
						if (cpd == null)
							throw new BulletException();

						cpd.JacDiagABInv = 1f / jacDiagAB;

						//Dependent on Rigidbody A and B types, fetch the contact/friction response func
						//perhaps do a similar thing for friction/restutution combiner funcs...

						cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
						cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];

						Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
						Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
						Vector3 vel = vel1 - vel2;
						float rel_vel;
						rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);

						float combinedRestitution = cp.CombinedRestitution;

						cpd.Penetration = cp.Distance;
						cpd.Friction = cp.CombinedFriction;
						cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
						if (cpd.Restitution < 0f)
						{
							cpd.Restitution = 0.0f;

						};

						//restitution and penetration work in same direction so
						//rel_vel 

						float penVel = -cpd.Penetration / info.TimeStep;

						if (cpd.Restitution > penVel)
						{
							cpd.Penetration = 0;
						}


						float relaxation = info.Damping;
						if ((_solverMode & SolverMode.UseWarmstarting) != 0)
						{
							cpd.AppliedImpulse *= relaxation;
						}
						else
						{
							cpd.AppliedImpulse = 0f;
						}

						//for friction
						cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;

						//re-calculate friction direction every frame, todo: check if this is really needed
						Vector3 fwta = cpd.FrictionWorldTangentialA;
						Vector3 fwtb = cpd.FrictionWorldTangentialB;
						MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
						cpd.FrictionWorldTangentialA = fwta;
						cpd.FrictionWorldTangentialB = fwtb;

						cpd.AccumulatedTangentImpulseA = 0;
						cpd.AccumulatedTangentImpulseB = 0;
						float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
						float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
						float denom = relaxation / (denom0 + denom1);
						cpd.JacDiagABInvTangentA = denom;


						denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
						denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
						denom = relaxation / (denom0 + denom1);
						cpd.JacDiagABInvTangentB = denom;

						Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;

						{
							Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
							cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
							Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
							cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
							cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
							cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
							cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
						}
						{
							Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
							cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
						}


						//apply previous frames impulse on both bodies
						body0.ApplyImpulse(totalImpulse, rel_pos1);
						body1.ApplyImpulse(-totalImpulse, rel_pos2);
					}
				}
			}
		}

		private bool MyContactDestroyedCallback(object userPersistentData)
		{
			if (userPersistentData == null)
				throw new BulletException();
			ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
			_totalCpd--;
			return true;
		}

		private float RestitutionCurve(float relVel, float restitution)
		{
			float rest = restitution * -relVel;
			return rest;
		}

		//velocity + friction
		//response  between two dynamic objects with friction
		public virtual float ResolveSingleCollisionCombinedCacheFriendly(
			SolverBody bodyA,
			SolverBody bodyB,
			SolverConstraint contactConstraint,
			ContactSolverInfo solverInfo)
		{
			float normalImpulse = 0;

			if (contactConstraint.Penetration < 0)
				return 0;

			float relVel;
			float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
						+ Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
			float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
						+ Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);

			relVel = velADotn - velBDotn;

			float positionalError = contactConstraint.Penetration;
			float velocityError = contactConstraint.Restitution - relVel;// * damping;

			float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
			float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
			normalImpulse = penetrationImpulse + velocityImpulse;

			// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
			float oldNormalImpulse = contactConstraint.AppliedImpulse;
			float sum = oldNormalImpulse + normalImpulse;
			contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;

			float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
			float velocitySum = oldVelocityImpulse + velocityImpulse;
			contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;

			normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;

			if (bodyA.InvMass != 0)
			{
				bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
					contactConstraint.AngularComponentA, normalImpulse);
			}
			if (bodyB.InvMass != 0)
			{
				bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
					contactConstraint.AngularComponentB, -normalImpulse);
			}

			return normalImpulse;
		}

		public virtual float ResolveSingleFrictionCacheFriendly(
			SolverBody bodyA,
			SolverBody bodyB,
			SolverConstraint contactConstraint,
			ContactSolverInfo solverInfo,
			float appliedNormalImpulse)
		{
			float combinedFriction = contactConstraint.Friction;
			float limit = appliedNormalImpulse * combinedFriction;

			if (appliedNormalImpulse > 0)
			//friction
			{
				float j1;
				{
					float relVel;
					float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
								+ Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
					float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
						+ Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
					relVel = velADotn - velBDotn;

					// calculate j that moves us to zero relative velocity
					j1 = -relVel * contactConstraint.JacDiagABInv;
					float oldTangentImpulse = contactConstraint.AppliedImpulse;
					contactConstraint.AppliedImpulse = oldTangentImpulse + j1;

					float test = contactConstraint.AppliedImpulse;
					MathHelper.SetMin(ref test, limit);
					MathHelper.SetMax(ref test, -limit);
					contactConstraint.AppliedImpulse = test;

					j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
				}

				if (bodyA.InvMass != 0)
				{
					bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
				}
				if (bodyB.InvMass != 0)
				{
					bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
				}
			}
			return 0;
		}

		public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
		{
			if (constraints.Count + numManifolds == 0)
			{
				return 0;
			}

			for (int i = 0; i < numManifolds; i++)
			{
				PersistentManifold manifold = manifolds[i];
				RigidBody rbA = (RigidBody)manifold.BodyA;
				RigidBody rbB = (RigidBody)manifold.BodyB;

				manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);
			}

			int minReservation = manifolds.Count * 2;

			_tmpSolverBodyPool = new List<SolverBody>(minReservation);

			for (int i = 0; i < bodies.Count; i++)
			{
				RigidBody rb = RigidBody.Upcast(bodies[i]);
				if (rb != null && rb.IslandTag >= 0)
				{
					BulletDebug.Assert(rb.CompanionID < 0);
					int solverBodyId = _tmpSolverBodyPool.Count;
					SolverBody solverBody;
					InitSolverBody(out solverBody, rb);
					_tmpSolverBodyPool.Add(solverBody);
					rb.CompanionID = solverBodyId;
				}
			}

			_tmpSolverConstraintPool = new List<SolverConstraint>(minReservation);
			_tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation);

			for (int i = 0; i < numManifolds; i++)
			{
				PersistentManifold manifold = manifolds[i];
				RigidBody rb0 = (RigidBody)manifold.BodyA;
				RigidBody rb1 = (RigidBody)manifold.BodyB;

				int solverBodyIdA = -1;
				int solverBodyIdB = -1;

				//if (i == 89)
				//    System.Diagnostics.Debugger.Break();

				if (manifold.ContactsCount != 0)
				{
					if (rb0.IslandTag >= 0)
					{
						solverBodyIdA = rb0.CompanionID;
					}
					else
					{
						//create a static body
						solverBodyIdA = _tmpSolverBodyPool.Count;
						SolverBody solverBody;
						InitSolverBody(out solverBody, rb0);
						_tmpSolverBodyPool.Add(solverBody);
					}

					if (rb1.IslandTag >= 0)
					{
						solverBodyIdB = rb1.CompanionID;
					}
					else
					{
						//create a static body
						solverBodyIdB = _tmpSolverBodyPool.Count;
						SolverBody solverBody;
						InitSolverBody(out solverBody, rb1);
						_tmpSolverBodyPool.Add(solverBody);
					}
				}

				if (solverBodyIdB == -1 || solverBodyIdA == -1)
					System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));

				for (int j = 0; j < manifold.ContactsCount; j++)
				{
					ManifoldPoint cp = manifold.GetContactPoint(j);

					int frictionIndex = _tmpSolverConstraintPool.Count;

					if (cp.Distance <= 0)
					{

						Vector3 pos1 = cp.PositionWorldOnA;
						Vector3 pos2 = cp.PositionWorldOnB;

						Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;
						Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;

						float relaxation = 1;
						{
							SolverConstraint solverConstraint = new SolverConstraint();
							_tmpSolverConstraintPool.Add(solverConstraint);

							solverConstraint.SolverBodyIdA = solverBodyIdA;
							solverConstraint.SolverBodyIdB = solverBodyIdB;
							solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;

							//can be optimized, the cross products are already calculated
							float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);
							float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);
							float denom = relaxation / (denom0 + denom1);
							solverConstraint.JacDiagABInv = denom;

							solverConstraint.ContactNormal = cp.NormalWorldOnB;
							solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
							solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);

							Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);
							Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);

							Vector3 vel = vel1 - vel2;
							float rel_vel;
							rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);


							solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations);
							solverConstraint.Friction = cp.CombinedFriction;
							float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);
							if (rest <= 0)
							{
								rest = 0;
							}

							float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;
							if (rest > penVel)
							{
								rest = 0;
							}
							solverConstraint.Restitution = rest;

							solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);

							solverConstraint.AppliedImpulse = 0f;
							solverConstraint.AppliedVelocityImpulse = 0f;

#warning Check to see if we need Vector3.Transform
							Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
							solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);
							Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
							solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);
						}
						//create 2 '1d axis' constraints for 2 tangential friction directions

						//re-calculate friction direction every frame, todo: check if this is really needed
						Vector3 frictionTangential0a = new Vector3(),
								frictionTangential1b = new Vector3();

						MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);
						{
							SolverConstraint solverConstraint = new SolverConstraint();
							_tmpSolverFrictionConstraintPool.Add(solverConstraint);
							solverConstraint.ContactNormal = frictionTangential0a;

							solverConstraint.SolverBodyIdA = solverBodyIdA;
							solverConstraint.SolverBodyIdB = solverBodyIdB;
							solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
							solverConstraint.FrictionIndex = frictionIndex;

							solverConstraint.Friction = cp.CombinedFriction;

							solverConstraint.AppliedImpulse = 0;
							solverConstraint.AppliedVelocityImpulse = 0;

							float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
							float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
							float denom = relaxation / (denom0 + denom1);
							solverConstraint.JacDiagABInv = denom;

							{
								Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
								solverConstraint.RelPosACrossNormal = ftorqueAxis0;
								solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);
							}
							{
								Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
								solverConstraint.RelPosBCrossNormal = ftorqueAxis0;
								solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);
							}
						}


						{

							SolverConstraint solverConstraint = new SolverConstraint();
							_tmpSolverFrictionConstraintPool.Add(solverConstraint);
							solverConstraint.ContactNormal = frictionTangential1b;

							solverConstraint.SolverBodyIdA = solverBodyIdA;
							solverConstraint.SolverBodyIdB = solverBodyIdB;
							solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
							solverConstraint.FrictionIndex = frictionIndex;

							solverConstraint.Friction = cp.CombinedFriction;

							solverConstraint.AppliedImpulse = 0;
							solverConstraint.AppliedVelocityImpulse = 0;

							float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
							float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
							float denom = relaxation / (denom0 + denom1);
							solverConstraint.JacDiagABInv = denom;
							{
								Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
								solverConstraint.RelPosACrossNormal = ftorqueAxis1;
								solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);
							}
							{
								Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
								solverConstraint.RelPosBCrossNormal = ftorqueAxis1;
								solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);
							}
						}
					}
				}
			}

			ContactSolverInfo info = infoGlobal;
			{
				for (int j = 0; j < constraints.Count; j++)
				{
					TypedConstraint constraint = constraints[j];
					constraint.BuildJacobian();
				}
			}

			int numConstraintPool = _tmpSolverConstraintPool.Count;
			int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;

			//todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
			List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool);
			List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool);
			{
				for (int i = 0; i < numConstraintPool; i++)
				{
					gOrderTmpConstraintPool.Add(i);
				}
				for (int i = 0; i < numFrictionPool; i++)
				{
					gOrderFrictionConstraintPool.Add(i);
				}
			}

			//should traverse the contacts random order...
			int iteration;
			{
				for (iteration = 0; iteration < info.IterationsCount; iteration++)
				{

					int j;
					if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
					{
						if ((iteration & 7) == 0)
						{
							for (j = 0; j < numConstraintPool; ++j)
							{
								int tmp = gOrderTmpConstraintPool[j];
								int swapi = RandInt2(j + 1);
								gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
								gOrderTmpConstraintPool[swapi] = tmp;
							}

							for (j = 0; j < numFrictionPool; ++j)
							{
								int tmp = gOrderFrictionConstraintPool[j];
								int swapi = RandInt2(j + 1);
								gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
								gOrderFrictionConstraintPool[swapi] = tmp;
							}
						}
					}

					for (j = 0; j < constraints.Count; j++)
					{
						TypedConstraint constraint = constraints[j];
						//todo: use solver bodies, so we don't need to copy from/to btRigidBody

						if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
						{
							_tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();
						}
						if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
						{
							_tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();
						}

						constraint.SolveConstraint(info.TimeStep);

						if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
						{
							_tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();
						}
						if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
						{
							_tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();
						}

					}

					{
						int numPoolConstraints = _tmpSolverConstraintPool.Count;
						for (j = 0; j < numPoolConstraints; j++)
						{
							SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
							ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
								_tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);
						}
					}

					{
						int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;
						for (j = 0; j < numFrictionPoolConstraints; j++)
						{
							SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
							float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;

							ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
								_tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);
						}
					}
				}
			}

			for (int i = 0; i < _tmpSolverBodyPool.Count; i++)
			{
				_tmpSolverBodyPool[i].WriteBackVelocity();
			}

			_tmpSolverBodyPool.Clear();
			_tmpSolverConstraintPool.Clear();
			_tmpSolverFrictionConstraintPool.Clear();

			return 0;
		}

		public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
		{
			if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
			{
				return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);
			}

			ContactSolverInfo info = infoGlobal;
			int totalPoints = 0;

			int numiter = infoGlobal.IterationsCount;

			for (int j = 0; j < manifolds.Count; j++)
			{
				PersistentManifold manifold = manifolds[j];
				PrepareConstraints(manifold, info);

				for (int p = 0; p < manifolds[j].ContactsCount; p++)
				{
					_order[totalPoints].ManifoldIndex = j;
					_order[totalPoints].PointIndex = p;
					totalPoints++;
				}
			}

			for (int j = 0; j < constraints.Count; j++)
			{
				constraints[j].BuildJacobian();
			}

			//should traverse the contacts random order...
			int iteration;

			for (iteration = 0; iteration < numiter; iteration++)
			{
				int j;
				if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
				{
					if ((iteration & 7) == 0)
					{
						for (j = 0; j < totalPoints; ++j)
						{
							OrderIndex tmp = _order[j];
							int swapi = RandInt2(j + 1);
							_order[j] = _order[swapi];
							_order[swapi] = tmp;
						}
					}
				}

				for (j = 0; j < constraints.Count; j++)
				{
					constraints[j].SolveConstraint(info.TimeStep);
				}

				for (j = 0; j < totalPoints; j++)
				{
					PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
					Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
						manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
				}

				for (j = 0; j < totalPoints; j++)
				{
					PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
					SolveFriction((RigidBody)manifold.BodyA,
						(RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
				}
			}

			return 0;
		}

		private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
		{
			solverBody = new SolverBody();
			solverBody.AngularVelocity = rigidBody.AngularVelocity;
			solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
			solverBody.Friction = rigidBody.Friction;
			solverBody.InvMass = rigidBody.InverseMass;
			solverBody.LinearVelocity = rigidBody.LinearVelocity;
			solverBody.OriginalBody = rigidBody;
			solverBody.AngularFactor = rigidBody.AngularFactor;
		}

		private long Rand2()
		{
			_seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;
			return _seed2;
		}

		private int RandInt2(int n)
		{
			// seems good; xor-fold and modulus
			long un = n;
			long r = Rand2();

			// note: probably more aggressive than it needs to be -- might be
			//       able to get away without one or two of the innermost branches.
			if (un <= 0x00010000L)
			{
				r ^= (r >> 16);
				if (un <= 0x00000100L)
				{
					r ^= (r >> 8);
					if (un <= 0x00000010L)
					{
						r ^= (r >> 4);
						if (un <= 0x00000004L)
						{
							r ^= (r >> 2);
							if (un <= 0x00000002L)
							{
								r ^= (r >> 1);
							}
						}
					}
				}
			}
			return (int)(r % un);
		}

		protected struct OrderIndex
		{
			private int _manifoldIndex;
			private int _pointIndex;

			public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }
			public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } }
		}
	}
}
